/*
 * MotorController.cpp
 *
 *  Created on: Nov 13, 2013
 *      Author: Kellen Carey
 */

/*Belongs to MotionPlanner*/
#include "../include/MotorController.h"
#include <string>
#include <sstream>
#include <stdlib.h>
#include <vector>
#include <iomanip>
#include <cmath>
#include <iostream>
#include <fstream>
#include <map>

#include "dynamixel.h"
using namespace Robot;

MotorController* MotorController::m_UniqueInstance = new MotorController();


#define ID					(2)
#define LENGTH				(3)
#define INSTRUCTION			(4)
#define ERRBIT				(4)
#define PARAMETER			(5)
#define DEFAULT_BAUDNUMBER	(1)

#define INST_PING			(1)
#define INST_READ			(2)
#define INST_WRITE			(3)
#define INST_REG_WRITE		(4)
#define INST_ACTION			(5)
#define INST_RESET			(6)
#define INST_SYNC_WRITE		(131)   // 0x83
#define INST_BULK_READ      (146)   // 0x92

std::vector<int> motors;
std::vector<int> data;

// Default Constructor
MotorController::MotorController() {
	// Initialize USB2Dynamixel
	//	motor_dynamixel.dxl_initialize(0,1);
	dxl_initialize(0,1);
	//Initialize the motors
	initialize();
}

// Default Destructor
MotorController::~MotorController() {
	//	motor_dynamixel.dxl_terminate();
	dxl_terminate();
}
void MotorController::initialize() {

	setStatusReturnLevel(STATUS_RETURN);
	//std::cout<<"Success Initializing USB2AX\n"<<std::endl;
}
//int MotorController::ReadWord(int id, int address, int *pValue, int *error)
//{
//	unsigned char txpacket[MAXNUM_TXPARAM + 10] = {0, };
//	unsigned char rxpacket[MAXNUM_RXPARAM + 10] = {0, };
//	int result;
//
//    txpacket[ID]           = (unsigned char)id;
//    txpacket[INSTRUCTION]  = INST_READ;
//	txpacket[PARAMETER]    = (unsigned char)address;
//    txpacket[PARAMETER+1]  = 2;
//    txpacket[LENGTH]       = 4;
//
//	result = TxRxPacket(txpacket, rxpacket, 2);
//	if(result == SUCCESS)
//	{
//		*pValue = MakeWord((int)rxpacket[PARAMETER], (int)rxpacket[PARAMETER + 1]);
//
//		if(error != 0)
//			*error = (int)rxpacket[ERRBIT];
//	}
//
//	return result;
//}

void MotorController::sendSyncWrite(std::vector<int> ids, int address, int instruction_length, std::vector<int> data) {
	// Make syncwrite packet
	int idsLength= ids.size();

	dxl_set_txpacket_id(BROADCAST_ID);//ID of destination
	dxl_set_txpacket_instruction(INST_SYNC_WRITE);// packet type
	dxl_set_txpacket_parameter(0, address);//which data to manipulate. speed, goal pos, ect
	dxl_set_txpacket_parameter(1, instruction_length);//how long the instruction will be. 2 for word, 1 for byte,
	for(int i=0; i<idsLength; i++ )
	{
		// Set the ID number
		dxl_set_txpacket_parameter(2+(instruction_length+1)*i, ids[i]);
		// Set the data values
		if (instruction_length == 1) {
			dxl_set_txpacket_parameter(2+2*i+1, data[i]);
		}

		else if (instruction_length == 2) {
			dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(data[i]));
			dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(data[i]));
		}
	}
	dxl_set_txpacket_length((instruction_length+1)*idsLength+4);//(2+1) for writing a word, (1+1) for writing a byte, 4 is a constant

	dxl_txrx_packet();//sends the packet
	int result = dxl_get_result( );

//	if( result == COMM_TXSUCCESS )
//
//	{
//		std::cout << "COMM_TXSUCCESS "<<std::endl;
//	}
//
//	else if( result == COMM_RXSUCCESS )
//
//	{
//		std::cout << " "<<std::endl;
//	}
//
//	else if( result == COMM_TXFAIL )
//
//	{
//		std::cout << "COMM_TXFAIL "<<std::endl;
//	}
//
//	else if( result == COMM_RXFAIL)
//
//	{
//		std::cout << "COMM_RXFAIL "<<std::endl;
//	}
//
//	else if( result == COMM_TXERROR )
//
//	{
//		std::cout << "COMM_TXERROR "<<std::endl;
//	}
//
//	else if( result == COMM_RXWAITING )
//
//	{
//		std::cout << "COMM_RXWAITING "<<std::endl;
//	}
}
void MotorController::SyncWrite(int start_addr, int each_length, int number, int *pParam)
{
	unsigned char txpacket[MAXNUM_TXPARAM + 10] = {0, };
	unsigned char rxpacket[MAXNUM_RXPARAM + 10] = {0, };
	int n;

	dxl_set_txpacket_id(ID_BROADCAST);//ID of destination
	dxl_set_txpacket_instruction(INST_SYNC_WRITE);// packet type
	dxl_set_txpacket_parameter(0, start_addr);//which data to manipulate. speed, goal pos, ect
	dxl_set_txpacket_parameter(1, each_length-1);//how long the instruction will be. 2 for word, 1 for byte,

	//    txpacket[ID]                = (unsigned char)ID_BROADCAST;
	//    txpacket[INSTRUCTION]       = INST_SYNC_WRITE;
	//    txpacket[PARAMETER]			= (unsigned char)start_addr;
	//    txpacket[PARAMETER + 1]		= (unsigned char)(each_length - 1);
	for(n = 0; n < (number * each_length); n++){
		txpacket[PARAMETER + 2 + n]   = (unsigned char)pParam[n];
	}
	dxl_set_txpacket_length(n+4);
	// txpacket[LENGTH]  			= n + 4;
	dxl_txrx_packet();//sends the packet
	//return TxRxPacket(txpacket, rxpacket, 0);
	dxl_txrx_packet();//sends the packet

	int result = dxl_get_result( );

	if( result == COMM_TXSUCCESS )

	{
		std::cout << "COMM_TXSUCCESS "<<std::endl;
	}

	else if( result == COMM_RXSUCCESS )

	{
		std::cout << " "<<std::endl;
	}

	else if( result == COMM_TXFAIL )

	{
		std::cout << "COMM_TXFAIL "<<std::endl;
	}

	else if( result == COMM_RXFAIL)

	{
		std::cout << "COMM_RXFAIL "<<std::endl;
	}

	else if( result == COMM_TXERROR )

	{
		std::cout << "COMM_TXERROR "<<std::endl;
	}

	else if( result == COMM_RXWAITING )

	{
		std::cout << "COMM_RXWAITING "<<std::endl;
	}
}


int MotorController::getMotorPositionReadWord(int id) {
	return dxl_read_word(id, PRESENT_POSITION);
}


void MotorController::changePID(int motor, int newPID) {
	if(motor==0){

		for(int i= 0; i<TOTAL_MOTORS; i++){
			dxl_write_byte(i+1, newPID, P_FOR_PID);
		}

	}
	else{

		dxl_write_byte(motor, newPID, P_FOR_PID);
	}
}


int MotorController::readMotorPosition(int motor) {
	return ( (int)dxl_read_word(motor, PRESENT_POSITION) );
}
void MotorController::setStatusReturnLevel(int level) {
	for (int i = 0; i < NUM_MOTORS; i++) {
		dxl_write_byte(i+1, STATUS_RETURN_LEVEL, level);
	}

}

void MotorController::setMotorLimits(int motor, int CW, int CCW){
	dxl_write_word(motor, CW_LIMIT, CW);
	dxl_write_word(motor, CCW_LIMIT, CCW);


}

